#include "board.h"
#include "hpm_gpio_drv.h"
#include "string.h"
#include "app_ethcat_master.h"
#include "thread.h"
#include "hpm_gptmr_drv.h"
#define SYNC0TIME 10000   //8000

static void init_gptmr0(uint32_t ms)
{
    uint32_t gptmr_freq;
    gptmr_channel_config_t config;

    gptmr_channel_get_default_config(HPM_GPTMR0, &config);

    clock_add_to_group(clock_gptmr0, 0);
    gptmr_freq = clock_get_frequency(clock_gptmr0);

    config.reload = gptmr_freq / 1000 * ms;
    gptmr_channel_config(HPM_GPTMR0, 0, &config, false);
    gptmr_enable_irq(HPM_GPTMR0, GPTMR_CH_RLD_IRQ_MASK(0));
    intc_m_enable_irq_with_priority(IRQn_GPTMR0, 3);

    gptmr_start_counter(HPM_GPTMR0, 0);
}

static void thread_tick_isr(void)
{
    if (gptmr_check_status(HPM_GPTMR0, GPTMR_CH_RLD_STAT_MASK(0))) {
        gptmr_clear_status(HPM_GPTMR0, GPTMR_CH_RLD_STAT_MASK(0));
        Thread_RunCheck();
    }
}
SDK_DECLARE_EXT_ISR_M(IRQn_GPTMR0, thread_tick_isr);

uint32 network_configuration(void)
{
   ///* Do we got expected number of slaves from config */
   //if (ec_slavecount < NUMBER_OF_SLAVES)
   //   return 0;

   ///* Verify slave by slave that it is correct*/
   //if (strcmp(ec_slave[EK1100_1].name,"EK1100"))
   //   return 0;
   //else if (strcmp(ec_slave[EL4001_1].name,"EL4001"))
   //   return 0;
   //else if (strcmp(ec_slave[EL3061_1].name,"EL3061"))
   //   return 0;
   //else if (strcmp(ec_slave[EL1008_1].name,"EL1008"))
   //   return 0;
   //else if (strcmp(ec_slave[EL1008_2].name,"EL1008"))
   //   return 0;
   //else if (strcmp(ec_slave[EL2622_1].name,"EL2622"))
   //   return 0;
   //else if (strcmp(ec_slave[EL2622_2].name,"EL2622"))
   //   return 0;
   //else if (strcmp(ec_slave[EL2622_3].name,"EL2622"))
   //   return 0;
   //else if (strcmp(ec_slave[EL2622_4].name,"EL2622"))
   //   return 0;

  return 1;
}

char IOmap[512];
int dorun = 0;
PDO_Output *outputs1;
PDO_Input *inputs1;
int Servosetup(uint16 slave)
{
    int retval;
    uint16 u16val;
    uint8  u8val;
    uint32 u32val;
    retval = 0;

    retval += ec_SDOwrite(slave, 0x1c12, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM);//RXPDO分配对象字典为0x1c12 将起子索引0设置为0代表停止RXPDO分配功能
    u16val = 0x1600;
    retval += ec_SDOwrite(slave, 0x1c12, 0x01, FALSE, sizeof(u16val), &u16val, EC_TIMEOUTRXM);//设置TXPDO分配对象，即为0x1a00，0x1600为RXPDO映射对象组1
    u8val = 1;
    retval += ec_SDOwrite(slave, 0x1c12, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM);//重新打开RXPDO分配功能，设置 0x1C12的子索引 0 为 1,也就是只有1个对象组


    u8val = 0;//RX相对驱动器从机来说就是接收，对主机来说是发送
    retval += ec_SDOwrite(slave, 0x1600, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM);//RXPDO映射对象组1的对象字典为0x1600,其子索引0设为0代表停止其映射功能

    u32val = 0x70000110;//0x7000对象字典的含义是驱动器的控制字，00代表该对象字典的subitem，最后10为该对象字典的BIT长度，0x10即为16BIT，也就是2个字节
    retval += ec_SDOwrite(slave, 0x1600, 0x01, FALSE, sizeof(u32val), &u32val, EC_TIMEOUTRXM);//往TXPDO映射对象组1的子索引1写入映射对象,0x70000110

    u32val = 0x70000210;
    retval += ec_SDOwrite(slave, 0x1600, 0x02, FALSE, sizeof(u32val), &u32val, EC_TIMEOUTRXM);

    u32val = 0x70000310;
    retval += ec_SDOwrite(slave, 0x1600, 0x03, FALSE, sizeof(u32val), &u32val, EC_TIMEOUTRXM);

    u32val = 0x70000410;
    retval += ec_SDOwrite(slave, 0x1600, 0x04, FALSE, sizeof(u32val), &u32val, EC_TIMEOUTRXM);

    u32val = 0x70000501;
    retval += ec_SDOwrite(slave, 0x1600, 0x05, FALSE, sizeof(u32val), &u32val, EC_TIMEOUTRXM);

    u32val = 0x70000601;
    retval += ec_SDOwrite(slave, 0x1600, 0x06, FALSE, sizeof(u32val), &u32val, EC_TIMEOUTRXM);

    u32val = 0x70000701;
    retval += ec_SDOwrite(slave, 0x1600, 0x07, FALSE, sizeof(u32val), &u32val, EC_TIMEOUTRXM);

    u32val = 0x70000801;
    retval += ec_SDOwrite(slave, 0x1600, 0x08, FALSE, sizeof(u32val), &u32val, EC_TIMEOUTRXM);

    u32val = 0x70000901;
    retval += ec_SDOwrite(slave, 0x1600, 0x09, FALSE, sizeof(u32val), &u32val, EC_TIMEOUTRXM);

    u32val = 0x70000a01;
    retval += ec_SDOwrite(slave, 0x1600, 0x0a, FALSE, sizeof(u32val), &u32val, EC_TIMEOUTRXM);

    u32val = 0x70000b01;
    retval += ec_SDOwrite(slave, 0x1600, 0x0b, FALSE, sizeof(u32val), &u32val, EC_TIMEOUTRXM);

    u32val = 0x70000c01;
    retval += ec_SDOwrite(slave, 0x1600, 0x0c, FALSE, sizeof(u32val), &u32val, EC_TIMEOUTRXM);

    u8val = 12;//写入映射对象组1的对象总数为3
    retval += ec_SDOwrite(slave, 0x1600, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM); //设置映射对象组的对象总数

    retval += ec_SDOwrite(slave, 0x1c13, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM); //TXPDO分配对象字典为0x1c13 将起子索引0设置为0代表停止TXPDO分配功能
    u16val = 0x1A00;
    retval += ec_SDOwrite(slave, 0x1c13, 0x01, FALSE, sizeof(u16val), &u16val, EC_TIMEOUTRXM);//设置TXPDO分配对象，即为0x1a00，0x1a00为TXPDO映射对象组1
    u8val = 1;
    retval += ec_SDOwrite(slave, 0x1c13, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM);//重新打开TXPDO分配功能，设置 0x1C12 和 0x1C13 的子索引 0 为 1

    u8val = 0; //TX相对驱动器从机来说就是发送，对主机来说是接收
    retval += ec_SDOwrite(slave, 0x1A00, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM); //TXPDO分配对象字典为0x1A00 将起子索引0设置为0代表停止TXPDO分配功能
    u32val = 0x60000110;
    retval += ec_SDOwrite(slave, 0x1A00, 0x01, FALSE, sizeof(u32val), &u32val, EC_TIMEOUTRXM);
    u32val = 0x60000210;
    retval += ec_SDOwrite(slave, 0x1A00, 0x02, FALSE, sizeof(u32val), &u32val, EC_TIMEOUTRXM);

    u32val = 0x60000310;
    retval += ec_SDOwrite(slave, 0x1A00, 0x03, FALSE, sizeof(u32val), &u32val, EC_TIMEOUTRXM);

    u32val = 0x60000410;
    retval += ec_SDOwrite(slave, 0x1A00, 0x04, FALSE, sizeof(u32val), &u32val, EC_TIMEOUTRXM);

    u32val = 0x60000501;
    retval += ec_SDOwrite(slave, 0x1A00, 0x05, FALSE, sizeof(u32val), &u32val, EC_TIMEOUTRXM);

    u32val = 0x60000601;
    retval += ec_SDOwrite(slave, 0x1A00, 0x06, FALSE, sizeof(u32val), &u32val, EC_TIMEOUTRXM);

    u32val = 0x60000701;
    retval += ec_SDOwrite(slave, 0x1A00, 0x07, FALSE, sizeof(u32val), &u32val, EC_TIMEOUTRXM);

    u32val = 0x60000801;
    retval += ec_SDOwrite(slave, 0x1A00, 0x08, FALSE, sizeof(u32val), &u32val, EC_TIMEOUTRXM);

    u32val = 0x60000901;
    retval += ec_SDOwrite(slave, 0x1A00, 0x09, FALSE, sizeof(u32val), &u32val, EC_TIMEOUTRXM);

    u32val = 0x60000a01;
    retval += ec_SDOwrite(slave, 0x1A00, 0x0a, FALSE, sizeof(u32val), &u32val, EC_TIMEOUTRXM);

    u32val = 0x60000b01;
    retval += ec_SDOwrite(slave, 0x1A00, 0x0b, FALSE, sizeof(u32val), &u32val, EC_TIMEOUTRXM);

    u32val = 0x60000c01;
    retval += ec_SDOwrite(slave, 0x1A00, 0x0c, FALSE, sizeof(u32val), &u32val, EC_TIMEOUTRXM);

    u8val = 12;//写入映射对象组1的对象总数
    retval += ec_SDOwrite(slave, 0x1A00, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM); //设置映射对象组的对象总数

    return 1;
}

static void thread_ecat_rxd(void)
{
    if(dorun == 1)
    {
        ec_send_processdata();
        ec_receive_processdata(EC_TIMEOUTRET);
    }
}

static void thread_ecat_led(void)
{
    static uint8_t flag = 0;
    if(dorun == 1)
    {
        printf("in1:%d in2:%d %d %d \n",inputs1->IN_GEN_INT1,inputs1->IN_GEN_INT2,inputs1->IN_GEN_Bit1,inputs1->IN_GEN_Bit2);
       
        gpio_write_pin(BOARD_R_GPIO_CTRL, BOARD_R_GPIO_INDEX, BOARD_R_GPIO_PIN, inputs1->IN_GEN_Bit1);             
        gpio_write_pin(BOARD_B_GPIO_CTRL, BOARD_B_GPIO_INDEX, BOARD_B_GPIO_PIN, inputs1->IN_GEN_Bit2);          
        
        if(flag >= 8)
        {
            outputs1->OUT_GEN_Bit1 = 0;
            outputs1->OUT_GEN_Bit2 = 0;
            outputs1->OUT_GEN_Bit3 = 0;
            outputs1->OUT_GEN_Bit4 = 0;
            outputs1->OUT_GEN_Bit5 = 0;
            outputs1->OUT_GEN_Bit6 = 0;
            outputs1->OUT_GEN_Bit7 = 0;
            outputs1->OUT_GEN_Bit8 = 0;
            flag = 0;
        }
        switch(flag)
        {
          case 0:
            outputs1->OUT_GEN_Bit1 = 1;
            break;

          case 1:
            outputs1->OUT_GEN_Bit2 = 1;
            break;

          case 2:
           outputs1->OUT_GEN_Bit3 = 1;
            break;

          case 3:
            outputs1->OUT_GEN_Bit4 = 1;
            break;

           case 4:
            outputs1->OUT_GEN_Bit5 = 1;
            break;

           case 5:
            outputs1->OUT_GEN_Bit6 = 1;
            break;

           case 6:
            outputs1->OUT_GEN_Bit7 = 1;
            break;

           case 7:
            outputs1->OUT_GEN_Bit8 = 1;
            break;
        }
        flag++;
    }
}

static void xmc4800_test(void )
{
   int cnt, i, j,slc;

   printf("Starting simple test\n");

   /* initialise SOEM */
   if (ec_init() > 0)
   {
      printf("ec_init succeeded.\n");

      /* find and auto-config slaves */
      if ( ec_config_init(FALSE) > 0 )
      {
         printf("%d slaves found and configured.\n",ec_slavecount);
#if 1
         /* Check network  setup */
         if (network_configuration())
         {
            for(slc = 1; slc <= ec_slavecount; slc++)
            {
                 printf("Found %s at position %d\n", ec_slave[slc].name, slc);
                 ec_slave[slc].PO2SOconfig = &Servosetup;
            }
            slc -= 1;
            ec_configdc();
            ec_dcsync0(0, TRUE, SYNC0TIME, 250000); // SYNC0 on slave 1

            /* Run IO mapping */
            ec_config_map(&IOmap);

            printf("Slaves mapped, state to SAFE_OP.\n");
            /* wait for all slaves to reach SAFE_OP state */
            ec_statecheck(slc, EC_STATE_SAFE_OP,  EC_TIMEOUTSTATE);
            ec_readstate();
            printf("Slave 0 State=0x%04x\r\n",ec_slave[0].state);
            printf("Slave 1 State=0x%04x\r\n",ec_slave[1].state);

            /* Print som information on the mapped network */
            for( cnt = 1 ; cnt <= ec_slavecount ; cnt++)
            {
               printf("\nSlave:%d\n Name:%s\n Output size: %dbits\n Input size: %dbits\n State: %d\n Delay: %d[ns]\n Has DC: %d\n",
                       cnt, ec_slave[cnt].name, ec_slave[cnt].Obits, ec_slave[cnt].Ibits,
                       ec_slave[cnt].state, ec_slave[cnt].pdelay, ec_slave[cnt].hasdc);
               printf(" Configured address: %x\n", ec_slave[cnt].configadr);
               printf(" Outputs address: %x\n", ec_slave[cnt].outputs);
               printf(" Inputs address: %x\n", ec_slave[cnt].inputs);

               for(j = 0 ; j < ec_slave[cnt].FMMUunused ; j++)
               {
                  printf(" FMMU%1d Ls:%x Ll:%4d Lsb:%d Leb:%d Ps:%x Psb:%d Ty:%x Act:%x\n", j,
                          (int)ec_slave[cnt].FMMU[j].LogStart, ec_slave[cnt].FMMU[j].LogLength, ec_slave[cnt].FMMU[j].LogStartbit,
                          ec_slave[cnt].FMMU[j].LogEndbit, ec_slave[cnt].FMMU[j].PhysStart, ec_slave[cnt].FMMU[j].PhysStartBit,
                          ec_slave[cnt].FMMU[j].FMMUtype, ec_slave[cnt].FMMU[j].FMMUactive);
               }
               printf(" FMMUfunc 0:%d 1:%d 2:%d 3:%d\n",
                        ec_slave[cnt].FMMU0func, ec_slave[cnt].FMMU1func, ec_slave[cnt].FMMU2func, ec_slave[cnt].FMMU3func);

            }

            printf("Request operational state for all slaves\n");
            ec_slave[slc].state = EC_STATE_OPERATIONAL;
            ethcat_delay_us(20);
            /* send one valid process data to make outputs in slaves happy*/
            ec_send_processdata();
            ec_receive_processdata(EC_TIMEOUTRET);
            /* request OP state for all slaves */
            ec_writestate(slc);
            /* wait for all slaves to reach OP state */
             /* wait for all slaves to reach OP state */
            ec_statecheck(slc, EC_STATE_OPERATIONAL,  EC_TIMEOUTSTATE);
            do
            {
                ec_slave[slc].state = EC_STATE_OPERATIONAL;
                ec_send_processdata();
                ec_receive_processdata(EC_TIMEOUTRET);
                ec_statecheck(slc, EC_STATE_OPERATIONAL, 50000);
            }
            while (ec_slave[slc].state != EC_STATE_OPERATIONAL);
                  printf("Slave 0 State=0x%04x\r\n",ec_slave[slc].state);

            if (ec_slave[slc].state == EC_STATE_OPERATIONAL )
            {
               dorun = 1;
               outputs1 = (PDO_Output *)ec_slave[1].outputs;
               inputs1  = (PDO_Input *)ec_slave[1].inputs;
               printf("Operational state reached for all slaves.\n");
            }
            else
            {
               printf("Not all slaves reached operational state.\n");
               ec_readstate();
               for(i = 1; i<=ec_slavecount ; i++)
               {
                  if(ec_slave[i].state != EC_STATE_OPERATIONAL)
                  {
                     printf("Slave %d State=0x%04x StatusCode=0x%04x\n",
                             i, ec_slave[i].state, ec_slave[i].ALstatuscode);
                  }
               }
            }


            /* Simple blinking lamps BOX demo */
            //uint8 digout = 0;

            //slave_EL4001_1.out1 = (int16)0x3FFF;
            //set_output_int16(EL4001_1,0,slave_EL4001_1.out1);

            //task_spawn ("t_StatsPrint", my_cyclic_callback, 20, 1024, (void *)NULL);
            //tt_start_wait (tt_sched[0]);
            init_gptmr0(1);
            Thread_Login(FOREVER,0,50,&thread_ecat_rxd);
            Thread_Login(FOREVER,0,200,&thread_ecat_led);
            while(1)
            {
               Thread_Process(); 
               //if(dorun == 1)
               //{

               //   ec_send_processdata();
               //   ec_receive_processdata(EC_TIMEOUTRET);
               //   outputs1->OUT_GEN_Bit1 = 1;
               //   outputs1->OUT_GEN_Bit2 = 1;
               //   outputs1->OUT_GEN_Bit3 = 1;
               //   outputs1->OUT_GEN_Bit4 = 1;

               //}
               //ethcat_delay_us(50000);
            }
         }
         else
         {
            printf("Mismatch of network units!\n");
         }
#endif
      }
      else
      {
         printf("No slaves found!\n");
      }
      printf("End simple test, close socket\n");
      ethcat_delay_us(1500000);
      /* stop SOEM, close socket */
      //ec_close();
   }
   else
   {
      printf("ec_init failed");
   }
   printf("End program\n");
}



int main(void)
{
  board_init();
  board_init_led_pins();
  ethcat_init();

  while(1)
  {
    xmc4800_test();
  }
}